clc; clear; close all;

addpath('autogenV2\')

% 1、验证DH参数 passed
%{
alpha = [pi/2, -pi/2, pi/2, pi/2, pi/2];
a = [0, 0, 0.01491, 0, 0];

q = [pi/2, -1.5359, 0, pi, pi/2];
d = [0, 0, 0.27898, 0, 0.1085];

for i=1:5
    T(:,:,i)=[
            cos(q(i)), -sin(q(i)), 0, a(i);
            sin(q(i)).*cos(alpha(i)), cos(q(i))*cos(alpha(i)), -sin(alpha(i)), -d(i)*sin(alpha(i));
            sin(q(i))*sin(alpha(i)), cos(q(i))*sin(alpha(i)), cos(alpha(i)), d(i)*cos(alpha(i));
            0,0,0,1];
end

% 在axis_0坐标系下某点坐标
P = [-0.05896; 0.139; -0.28990; 1];
T01=T(:,:,1); P_a1 = inv(T01)*P; % 计算该点在axis_1坐标系下的值
T02=T01*T(:,:,2); P_a2 = inv(T02)*P; % 计算该点在axis_2坐标系下的值
T03=T02*T(:,:,3); P_a3 = inv(T03)*P; % 计算该点在axis_3坐标系下的值
T04=T03*T(:,:,4); P_a4 = inv(T04)*P; % 计算该点在axis_4坐标系下的值
T05=T04*T(:,:,5); P_a5 = inv(T05)*P; % 计算该点在axis_4坐标系下的值
%}

% 2、生成雅可比计算函数
%{
a = [0, 0, 0.01491, 0, 0];
d = [0, 0, 0.27898, 0, 0.1085];

% codegen
q=sym('q_%d',[5, 1],'real');
alpha = sym('alpha_%d',[5, 1],'real');

for i=1:5
    T(:,:,i)=[
            cos(q(i)), -sin(q(i)), 0, a(i);
            sin(q(i)).*cos(alpha(i)), cos(q(i))*cos(alpha(i)), -sin(alpha(i)), -d(i)*sin(alpha(i));
            sin(q(i))*sin(alpha(i)), cos(q(i))*sin(alpha(i)), cos(alpha(i)), d(i)*cos(alpha(i));
            0,0,0,1];
end

T01=T(:,:,1);
T02=T01*T(:,:,2);
T03=T02*T(:,:,3);
T04=T03*T(:,:,4);
T05=T04*T(:,:,5);

for (i=1: 4)
    for (j=1: 4)
        % t1
        u51(i, j) = jacobian(T05(i, j), q(1));
        u41(i, j) = jacobian(T04(i, j), q(1));
        u31(i, j) = jacobian(T03(i, j), q(1));
        u21(i, j) = jacobian(T02(i, j), q(1));
        u11(i, j) = jacobian(T01(i, j), q(1));
        
        % t2
        u52(i, j) = jacobian(T05(i, j), q(2));
        u42(i, j) = jacobian(T04(i, j), q(2));
        u32(i, j) = jacobian(T03(i, j), q(2));
        u22(i, j) = jacobian(T02(i, j), q(2));
        
        % t3
        u53(i, j) = jacobian(T05(i, j), q(3));
        u43(i, j) = jacobian(T04(i, j), q(3));
        u33(i, j) = jacobian(T03(i, j), q(3));
        
        % t4
        u54(i, j) = jacobian(T05(i, j), q(4)); 
        u44(i, j) = jacobian(T04(i, j), q(4));
        
        % t5
        u55(i, j) = jacobian(T05(i, j), q(5)); 
    end
end

optimize = true;
matlabFunction(u51, 'File', 'autogenV2/J_la_T05_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u41, 'File', 'autogenV2/J_la_T04_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u31, 'File', 'autogenV2/J_la_T03_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u21, 'File', 'autogenV2/J_la_T02_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u11, 'File', 'autogenV2/J_la_T01_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u52, 'File', 'autogenV2/J_la_T05_q2', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u42, 'File', 'autogenV2/J_la_T04_q2', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u32, 'File', 'autogenV2/J_la_T03_q2', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u22, 'File', 'autogenV2/J_la_T02_q2', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u53, 'File', 'autogenV2/J_la_T05_q3', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u43, 'File', 'autogenV2/J_la_T04_q3', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u33, 'File', 'autogenV2/J_la_T03_q3', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u54, 'File', 'autogenV2/J_la_T05_q4', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u44, 'File', 'autogenV2/J_la_T04_q4', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u55, 'File', 'autogenV2/J_la_T05_q5', 'Vars',{q, alpha}, 'Optimize', optimize);
%}

% 2、SW测量动力学参数
m1 = 0.1131;
r1_x=0.0;r1_y=-0.0007;r1_z=-0.0069;
m2 = 0.1016;
r2_x=0.0138;r2_y=0.0738;r2_z=0.0002;
m3 = 0.1588;
r3_x=0.0001;r3_y=-0.0003;r3_z=-0.0592;
m4 = 0.0982;
r4_x=0.0;r4_y=0.0636;r4_z=0.0003;
m5 = 0.0443;
r5_x=0.0;r5_y=0.0;r5_z=0.0180;

m1_r1 = m1*[r1_x;r1_y;r1_z;1];
m2_r2 = m2*[r2_x;r2_y;r2_z;1];
m3_r3 = m3*[r3_x;r3_y;r3_z;1];
m4_r4 = m4*[r4_x;r4_y;r4_z;1];
m5_r5 = m5*[r5_x;r5_y;r5_z;1];

% 3、读取采集数据，分析刚体动力学计算的力矩与采集力矩趋势是否相似？
data = load('dataset_v2/r_arm_01.txt');
% data的每一行包含的信息
% 1  2   3  4  5  6  7   8   9  10   11  12   13   14   15   16  17 18 19
% ts q1 q2 q3 q4 q5 dq1 dq2 dq3 dq4 dq5 tor1 tor2 tor3 tor4 tor5 gx gy gz

alpha = [pi/2, pi/2, -pi/2, -pi/2, -pi/2]';

% 提取其中的位置及力矩量
qs = data(:, 1:5);
tor = data(:, 11:15);

g = [0 0 -9.8 0]';
% gs = data(:, 17:19);

% 数据预处理
tor(:, 5) = tor(:, 5)*0.0;

% 计算理论力矩
Tau = [];

tau_1s = [];tau_2s = [];tau_3s = [];tau_4s = [];tau_5s = [];
tau_1s_est = [];tau_2s_est = [];tau_3s_est = [];tau_4s_est = [];tau_5s_est = [];
for i=1:1:length(qs)
    q = qs(i, :)'; 
   
    u11 = J_la_T01_q1(q, alpha);u21 = J_la_T02_q1(q, alpha);u31 = J_la_T03_q1(q, alpha);u41 = J_la_T04_q1(q, alpha);u51 = J_la_T05_q1(q, alpha);
    u22 = J_la_T02_q2(q, alpha);u32 = J_la_T03_q2(q, alpha);u42 = J_la_T04_q2(q, alpha);u52 = J_la_T05_q2(q, alpha);
    u33 = J_la_T03_q3(q, alpha);u43 = J_la_T04_q3(q, alpha);u53 = J_la_T05_q3(q, alpha);
    u44 = J_la_T04_q4(q, alpha);u54 = J_la_T05_q4(q, alpha);
    u55 = J_la_T05_q5(q, alpha);
    
    tau_1 = -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4-g'*u51*m5_r5;
    tau_2 = -g'*u22*m2_r2-g'*u32*m3_r3-g'*u42*m4_r4-g'*u52*m5_r5;
    tau_3 = -g'*u33*m3_r3-g'*u43*m4_r4-g'*u53*m5_r5;
    tau_4 = -g'*u44*m4_r4-g'*u54*m5_r5;
    tau_5 = -g'*u55*m5_r5;
    
    tau_5s = [tau_5s; tor(i, 5)];
    tau_4s = [tau_4s; tor(i, 4)];
    tau_3s = [tau_3s; tor(i, 3)];
    tau_2s = [tau_2s; tor(i, 2)];
    tau_1s = [tau_1s; tor(i, 1)];
    
    Tau = [Tau; 
        tor(i, 1)'; 
        tor(i, 2)'; 
        tor(i, 3)'; 
        tor(i, 4)'; 
        tor(i, 5)'
        ];
    
    tau_5s_est = [tau_5s_est; tau_5];
    tau_4s_est = [tau_4s_est; tau_4*0.5];
    tau_3s_est = [tau_3s_est; tau_3];
    tau_2s_est = [tau_2s_est; tau_2*4.0];
    tau_1s_est = [tau_1s_est; tau_1*4.0];
end

% plot
subplot(2, 3, 1);
plot(tau_1s, "r.-");
hold on;
plot(tau_1s_est, "g.-");
legend('real', 'est');   
subplot(2, 3, 2);
plot(tau_2s, "r.-");
hold on;
plot(tau_2s_est, "g.-");
legend('real', 'est');   
subplot(2, 3, 3);
plot(tau_3s, "r.-");
hold on;
plot(tau_3s_est, "g.-");
legend('real', 'est');   
subplot(2, 3, 4);
plot(tau_4s, "r-");
hold on;
plot(tau_4s_est, "g-");
legend('real', 'est');   
subplot(2, 3, 5);
plot(tau_5s, "r.-");
hold on;
plot(tau_5s_est, "g.-");
legend('real', 'est');   


